
/*****************************************************************************\
 *                          Testprogram for Grid
\*****************************************************************************/
 /*! @file
 *
 *  @brief
 *    Test for grid algorithm.
 *
 *  Type './grid -h' for an explanation of the command line options
 *  and keys.
 *
 *  If you want an interactive test program, use 'interactive'.
 *
 *  Tests:
 *  -#1  tests two identical spheres for intersection (should be true)
 *  -#2  tests two intersecting spheres for intersection (should be true)
 *  -#3  tests two not intersecting spheres for intersection (should be false)
 *  - tests if the moving algorithm for grids works correct and prints the cells
 *    of the grid wich include sphere 2.
 *
 *  @author Rene Weller
 *	- reimplemented by Weiyu Yi, wyi@tu-clausthal.de
 *
 *
 * @implementation
 *   This program is very similar to doptree.cpp and boxtree.cpp!
 *   So if you change something here, chances are high that you should change
 *   the same thing in doptree, too.
 */


//---------------------------------------------------------------------------
//  Includes
//---------------------------------------------------------------------------

#include <Collision.h>
#include <ColBoxtree.h>
#include <ColDopTree.h>
#include <ColTopology.h>
#include <ColUtils.h>
#include <ColObj.h>
#include <ColGrid.h>
#include <ColGridObj.h>
#include <ColGridCell.h>
#include "ColUtilsOpenGL.h"

using col::Point3;
using col::ColGeometry;
using col::Primitive;
using col::Cube;
using col::Icosahedron;

using col::Vector3;
using col::Vector4;
using col::Matrix4;
using col::Boxtree;
using col::DopTree;

namespace
{

static const unsigned int MaxNumCol = 10000;

typedef enum
{
	PERSPECTIVE,
	ORTHOGRAPHIC	
} ProjectionE;

struct PolygonPairs
{
	unsigned int npairs;
	static const unsigned int MaxNum = 10000;
	unsigned int pair[MaxNum][2][3];
	int error;
	PolygonPairs() : npairs(0), error(0) {};
};

// here declaration of the global variables.
col::ColObj *colobj[2];
ColGeometry *geo[2];
col::Grid *grid;
unsigned int gridsize;
Matrix4 moving_matrix;
col::AlgoE algo = col::ALGO_DOPTREE;
//col::AlgoE algo = col::ALGO_BOXTREE;

// render options
bool with_window = false;
ProjectionE proj = PERSPECTIVE;
GLenum fill_mode = GL_LINE;

GLdouble view_distance = 3.0;
col::Trackball tball;
Matrix4 viewing_matrix;
GLuint w, h;

GLdouble fixed_color[3] = { 1.0, 0.7, 0.7 };
GLdouble moving_color[3] = { 0.7, 0.7, 1.0 };

}

/***************************************************************************\
 *                        Test cases                                       *
\***************************************************************************/

/**  Translate a ColObj
 *
 * @param colobj        the collision obj
 * @param x, y, z       translation vector
 *
 **/
void moveColObj( col::ColObj *colobj, REAL x, REAL y, REAL z )
{
	// create a new matrix, which should be deleted by colobj.
	// Matrix4 does no initialization
	Matrix4 *m = new Matrix4();

	// assigned with identitiy matrix first
	(*m) = Matrix4::identity();
	(*m)[3][0] = x;
	(*m)[3][1] = y;
    (*m)[3][2] = z;
	
	// for rendering
	moving_matrix = (*m);

	// updating colobj
	colobj->setTransMatrix( m );
    colobj->updateGrid();
}

/**  Checks if the bounding volumes of the objects in the grid have intersections
 **/
bool checkIntersection ()
{
    vector<col::ColPair> colpairs;
    unsigned int neighbours = 0;
	grid->getCollPairs( &colpairs, &neighbours );
    if ( colpairs.empty() )
        return false;
    return true;
}

/**  Tests if move function from colgridobj.cpp works correct
 *
 * @param colobj        the collision obj
 * 
 * First traverse the gird cells and test if there exist a reference of the
 * colobj to a cell outside the bounding box.
 * Then check if a cell in the bounding box has no reference to the object
 * and print the visited cells
 **/
bool checkMove( col::ColObj *colobj )
{
    std::set<col::GridObj*, col::GridObjLtstr>*		objSetP;
    unsigned int min[3];
    unsigned int max[3];
    
    colobj->GetGridObj()->getBounds( min, max ); 
	//
	printf( "min: [% d % d % d]\nmax: [% d % d % d]\n", min[0], min[1], min[2], max[0], max[1], max[2] );

    for ( unsigned int x = 0; x < gridsize; x++ )
    for ( unsigned int y = 0; y < gridsize; y++ )
    for ( unsigned int z = 0; z < gridsize; z++ )
    {
		objSetP = grid->getCellP( x, y, z )->getObjSet();

        for ( std::set<col::GridObj*, col::GridObjLtstr>::iterator i = objSetP->begin();
			  i != objSetP->end(); i ++ )
		{
			if ( (*i)->getColObj() == colobj )
            {
                if( !( x >= min[0] && x <= max[0] &&
                    y >= min[1] && y <= max[1] &&
                    z >= min[2] && z <= max[2]) ) 
                {
                    printf ( "GridError: GridObj in Cell %d %d %d\n", x, y, z );
                    return false;
                }
            }
			
		}
    }

    bool found = false;

    for ( unsigned int x = min[0]; x <= max[0]; x++ )
    for ( unsigned int y = min[1]; y <= max[1]; y++ )
    for ( unsigned int z = min[2]; z <= max[2]; z++ )
    {
		objSetP = grid->getCellP( x, y, z )->getObjSet();

        for ( std::set<col::GridObj*, col::GridObjLtstr>::iterator i = objSetP->begin();
			  i != objSetP->end(); i ++ )
		{
            if ( (*i)->getColObj() == colobj )
            {
                found = true;
                printf ( "Object in GridCell: %d, %d, %d!\n", x, y, z );
            }
        
		}

        if( !found )
        {
            printf("GridError: GridObj not in Cell %d %d %d\n", x, y, z);
            return false;
        }

        found = false;
    }

    return true;
}

/// phase *must* start from 0 and increase step-wise by 1!
void testcase( int *phase )
{
try {
	int result = 0, result_wanted = -1;

	printf("\nphase %d\n", *phase );
	switch ( *phase )
	{
		case 0:
			moveColObj( colobj[0], 0, 0, 0 );
			moveColObj( colobj[1], 0, 0, 0 );

            if( grid->getNrCollPairs() > 0 )
                result = 1;
            else
                printf ( "GridError: No Intersection found\n" );
            result_wanted = 1;
			break;
		case 1:
            moveColObj( colobj[1], 1, 1, 1 );
            if( grid->getNrCollPairs() > 0 )
                result += 1;
            else
                printf ( "GridError: No Intersection found\n" );
            if( checkMove(colobj[1]) )
                result += 1;
            result_wanted = 2;
			break;
		case 2:
        {
            moveColObj( colobj[1], 7, 7, 7 );
            if( grid->getNrCollPairs() == 0 )
                result += 1;
            else
                printf ( "GridError: Intersection found\n" );
            if( checkMove(colobj[1]) )
                result += 1;
            result_wanted = 2;
            break;
		}
		default:
			// last phase has been reached, signal to caller
			printf( "last phase has been reached; starting again.\n" );
			*phase = -1;
	}
	(*phase) ++ ;

	if ( *phase )
	{
		if ( result_wanted < 0 )
			puts(".. done");
		else
		{
			if ( result_wanted == result )
				puts(".. ok");
			else
				puts(".. failed");
		}
	} // else: we just had a wrap-around, i.e., we didn't really do a test
}
catch ( col::XCollision &x )
{
	x.print(stderr);
	exit(-1);					// not sensible to continue testing
}
}

/**  Create 2 identical spheres
 * @a node[1] is actually a shallow copy of @a node[0], so that they will both
 * share the same geometry!
 * @a node[0] will be added under the root,
 * @a node[1] will be added under the 'moving_node' transformation node.
 * The number of polygons is usually something like @c 2*complexity^2 .
 *
 * If @a node[1] already contains a non-null node, then this will
 * be unlinked from the @c moving_node first, dito for the @a node[0].
 *
 * @todo
 * - In der createGeom-Fkt der anderen Testprogramme wird noch davon
 *   ausgegangen, dass FaceIterator auch n-gone (n>4) liefern kann.
 *   Auch dort aendern!
 *
 * @see
 *   testcases()
 *
 **/
void createGeom()
{
    unsigned int complexity = 4;
	complexity = static_cast<int>( log( static_cast<REAL>(complexity*complexity) ) );

	geo[0] = new ColGeometry();
	col::makeSphere( geo[0], complexity, 1.0 );
	//col::loadGeometryFromFile( geo[0], "../../../coll/wrapper/wrapper_opengl/test/Data/grid/grid_sphere_1.obj" );
	
	geo[1] = new ColGeometry();
	col::makeSphere( geo[1], complexity, 1.0 );
	//col::loadGeometryFromFile( geo[1], "../../../coll/wrapper/wrapper_opengl/test/Data/grid/grid_sphere_2.obj" );

    colobj[0] = new col::ColObj( 0, geo[0], false, false, false, algo, grid,
			false );

    colobj[1] = new col::ColObj( 1, geo[1], false, false, false, algo, grid,
			false );
}

void init(void) 
{
	glClearColor(0.0, 0.0, 0.0, 0.0);
	glShadeModel(GL_FLAT);
	glEnable(GL_DEPTH_TEST);
}

void reshape (int wide, int height)
{
	w = wide;
	h = height;
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	tball.setViewport( w, h );
}

void display(void)
{
	// all geometries are ready to render
	// begin display

	// trackball 
	if ( tball.getTrackMode() == col::TRACK_ROTATE )
	{
		REAL angle;
		REAL axis[3];
		tball.getRotation( axis, angle );
		Matrix4 m = Matrix4::rotation( angle, col::Vector3( axis[0], axis[1], axis[2] ) );
		viewing_matrix = m * viewing_matrix;
	}
	else
	if ( tball.getTrackMode() == col::TRACK_ZOOM )
	{
		REAL zoom;
		tball.getZoom( zoom );
		Matrix4 m = Matrix4::scale( col::Vector3( zoom+1 ) );
		viewing_matrix = m * viewing_matrix;
	}
	
	// 1) Prjection Trasformation
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity ();

	GLdouble range=2.0;
	switch( proj )
	{
		case ORTHOGRAPHIC:
			// orthographic projection
			if (w <= h)
				glOrtho(-1*range, range, -1*range*(GLdouble)h/(GLdouble)w,
						range*(GLdouble)h/(GLdouble)w, 1.0, 5.0);
			else
				glOrtho(-1*range*(GLdouble)w/(GLdouble)h,
						range*(GLdouble)w/(GLdouble)h, -1*range, range, 1.0, 5.0);
			break;
		case PERSPECTIVE:
			// perspective projection
			gluPerspective(60, (GLdouble)w/(GLdouble)h, 1.0, 5.0);
			break;
		default:
			fprintf( stderr, "No such projection mode!\n" );
			break;
	}

	// 2) View Transformation
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

	// parameter with "-" cause they are defined as eye->world, here is used as world-eye
	// care about the sequence of the commands
	
	// zoom out or zoom in
	glTranslated( 0.0, 0.0, -view_distance );

	GLdouble view_mat[16];
	for ( unsigned int i=0; i<4; i++ )
	for ( unsigned int j=0; j<4; j++ )
		view_mat[ i*4+j ] = viewing_matrix[i][j];
	glMultMatrixd( view_mat );

	// 3) Modelling Transformation

	// drawing
	if ( geo[0] != NULL )
	{
		glPushMatrix();
		col::drawGeometry( geo[0], fill_mode, fixed_color );
		glPopMatrix();
	}

	if ( geo[1] != NULL )
	{
		glPushMatrix();
		GLdouble mat[16];
		for ( unsigned int i=0; i<4; i++ )
			for ( unsigned int j=0; j<4; j++ )
				mat[ i*4+j ] = moving_matrix[i][j];
		glMultMatrixd( mat );

		col::drawGeometry( geo[1], fill_mode, moving_color );
		glPopMatrix();
	}

	//glFlush();
	glutSwapBuffers();
}

void mouseClick( int button, int state, int x, int y )
{
	tball.mouseClick( button, state, x, y );
}

void mouseMoving( int x, int y )
{
	tball.mouseMoving( x, y );
	glutPostRedisplay();
}

void keyboard(unsigned char key, int x, int y)
{
	static int phase = 0;

	switch (key) {
		case 27:
			exit(0);
			break;
		case ' ':
			testcase( &phase );
			glutPostRedisplay();
			break;
		case 'p':
		case 'P':
			if ( proj == PERSPECTIVE )
				proj = ORTHOGRAPHIC;
			else
			if ( proj == ORTHOGRAPHIC )
				proj = PERSPECTIVE;

			glutPostRedisplay();
			break;
		case 'i':
		case 'I':
			if ( fill_mode == GL_LINE )
				fill_mode = GL_FILL;
			else
			if ( fill_mode == GL_FILL )
				fill_mode = GL_POINT;
			else
			if ( fill_mode == GL_POINT )
				fill_mode = GL_LINE;

			glutPostRedisplay();
			break;
		default:
			break;
	}
}

/***************************************************************************\
 *                              Main                                       *
\***************************************************************************/

void commandlineerror( char *cmd, char *parm )
{
	if ( cmd )
		fprintf(stderr, "Offending option: %s\n", cmd );
	if ( parm )
		fprintf(stderr, "with first parameter: %s\n", parm );

	fprintf(stderr, "\n\nUsage: boxtree options ...\n"
	"Options:\n"
	"-w          with window; press space to continue with next test phase\n"
	"-h          show help\n"
	"Keys: (interactive version)\n"
	"i           polygon mode (wireframe, dots, filled)\n"
	"space       go to the next test case\n"
	"ESC         quit\n"
	"\n");

	if ( cmd )
		exit(-1);				// problem occured
	else
		exit(0);				// was option -h
}


void parsecommandline( int argc, char *argv[] )
{
	/* valid option characters; last char MUST be 0 ! */
	char optionchar[] =   { 'h', 'w', '\0' };
	int musthaveparam[] = {  0 ,  0  };
	int mhp[256];
	int isopt[256];
	char optchar;


	unsigned int nopts = strlen(optionchar);

	if ( nopts > 50 )
	{
		fprintf(stderr, "\n\nparsecommandline: the option-chars string "
				"seems to be\nVERY long (%d bytes) !\n\n", nopts );
		exit(-1);
	}

	fill_n( isopt, 256, 0 );
	fill_n( mhp, 256, 0 );
	for ( unsigned int i = 0; i < nopts; i ++ )
	{
		if ( isopt[static_cast<int>(optionchar[i])] )
		{
			fprintf(stderr, "\n\nparsecommandline: Bug: option character '%c'"
					" is specified twice in the\n"
							"option character array !\n\n", optionchar[i] );
			exit(-1);
		}
		isopt[ static_cast<int>(optionchar[i]) ] = 1;
		mhp[ static_cast<int>(optionchar[i])] = musthaveparam[i];
	}

	++argv; --argc;
	while ( argc > 0 )
	{
		if ( argv[0][0] == '-' )
		{
			optchar = argv[0][1];

			if ( ! isopt[static_cast<int>(optchar)] )
			{
				fprintf(stderr, "\nIs not a valid command line option\n");
				commandlineerror( argv[0], NULL );
			}
			for ( int i = 0; i < mhp[static_cast<int>(optchar)]; i ++ )
				if ( ! argv[1+i] || argv[1+i][0] == '-' )
				{
					fprintf(stderr, "\nCommand line option -%c must "
							"have %d parameter(s)\n",
							argv[0][1], mhp[static_cast<int>(optchar)] );
					commandlineerror( argv[0], NULL );
					argv += 1 + i;
					argc -= 1 + i;
					continue;
				}

			switch ( optchar )
			{
				case 'h': commandlineerror( NULL, NULL);  break;
				case 'w': with_window = true; break;
				default: fprintf(stderr, "\nBug in parsecommandline !\n");
						 commandlineerror( argv[0], NULL );
			}

			argv += 1 + mhp[static_cast<int>(optchar)];
			argc -= 1 + mhp[static_cast<int>(optchar)];
		}
		else
		{
			/* command line arg doesn't start with '-' */
			fprintf(stderr, "\nThis is not a valid command line option\n");
			commandlineerror( argv[0], NULL );
			/* or, load file instead .. */
		}
	}
}

int main(int argc, char** argv)
{
	col::DopTree::init();

    gridsize = 20;
	REAL cage_radius = 10.0;
    unsigned int size[3] = { gridsize, gridsize, gridsize };
	REAL min[3] = { 0, 0, 0 };
	REAL max[3] = { cage_radius, cage_radius, cage_radius };
    grid = new col::Grid( size, min, max );

    createGeom();    

	// parse command line options
	parsecommandline( argc, argv );

	// initialize global and static data
	DopTree::init();
	viewing_matrix = Matrix4::identity();

	// prepare glut
	glutInit( &argc, argv );
	glutInitDisplayMode ( GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB );
	glutInitWindowSize( 350, 350 ); 
	glutInitWindowPosition( 100, 100 );
	glutCreateWindow( argv[0] );
	init();

	// register glut callback
	glutDisplayFunc( display ); 
	glutReshapeFunc( reshape );
	glutMouseFunc( mouseClick );
	glutMotionFunc( mouseMoving );
	glutKeyboardFunc( keyboard );

	// run...
	if ( with_window )
		// render loop begins
		glutMainLoop();
	else
	{
		int phase = 0;
		do
		{
			testcase( &phase );
		}
		while ( phase != 0 );
	}

	return 0;
}

